
#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"

#include "basic_sprayer_interfaces/msg/adc.hpp"   //adc的Message
#include <iio.h>

#include <signal.h>
//#include <DspFilters/Butterworth.h>


using namespace std::chrono_literals;

#define IIO_ENSURE(expr) { \
	if (!(expr)) { \
		(void) fprintf(stderr, "assertion failed (%s:%d)\n", __FILE__, __LINE__); \
		(void) abort(); \
	} \
}

bool stop;     //停止信号
void sig_handler(int signum)
{
    if (signum == SIGINT || signum == SIGTERM) {
        stop = true;
    }
}

// //定义名字为name的参数默认值为Unknown
// this->declare_parameter<std::string>("name", "Unknown");
// //获取参数名字为name的值
// this->get_parameter("name", parameter_string_);
class AdcAnglePublisher : public rclcpp::Node
{
public:
    AdcAnglePublisher()
    : Node("adc_publisher")
    {
        // //定义名字为name的参数默认值为Unknown
        // this->declare_parameter<int16_t>("offsetf",2048);    // 前角度传感器中间值
        // this->declare_parameter<int16_t>("offsetr",2048);    // 后角度中间值
        // //获取参数名字为name的值
        // this->get_parameter("offsetf", parameter_string_);
        // this->get_parameter("offsetr", parameter_string_);
        // RCLCPP_INFO(this->get_logger(), "name: %s", parameter_string_.c_str());

        /* 建立iio设备的连接 */
        publisher_ = this->create_publisher<basic_sprayer_interfaces::msg::Adc>("adc", 10);        
        take_signals();                 // 接管信号
        ctx = NULL;
        adc = NULL;
        chna = NULL;
        chnb = NULL;
        time = NULL;
        trigger = NULL;
        rxbuf = NULL;
        stop = false;
        /* 初始化为空 */
        // filter = Dsp::FilterDesign<Dsp::Butterworth::Design::LowPass <2> >;
        // Dsp::Params params;
        // params[0] = 100; // sample rate
        // params[1] = 2; // order
        // params[2] = 4000; // cutoff frequency
        // /* 截止频率 */

        offset[0] = 1.668;
        offset[1] = 1.64;
    }

    void run(void)
    {
        ssize_t nbytes_rx;
        char *p_dat, *p_end;
        ptrdiff_t p_inc;

        if( iio_init() != true ){
            goto fail;
        }
        while(stop == false){
            nbytes_rx = iio_buffer_refill(rxbuf);
            if (nbytes_rx < 0) {
                printf("Error refilling buf: %d\n", (int)nbytes_rx);
                goto fail;
            }
            p_inc = iio_buffer_step(rxbuf);
            p_end = (char*)iio_buffer_end(rxbuf);
            for (p_dat = (char *)iio_buffer_first(rxbuf, chna); p_dat < p_end; p_dat += p_inc) {
                voltage[0] = (p_dat[1]<<8)|p_dat[0];
                voltage[1] = (p_dat[3]<<8)|p_dat[2];
                timestamp = *((int64_t *)(&p_dat[16-8]));
                // printf("voltage0=%fv,voltage1=%fv,time=%ld\r\n",
                // voltagex[0],voltagex[1],timestamp);
                ros_publish();           // 发布数据
            }
        }
        /* 关闭设备 */
        fail:
        shutdown();
    }

private:
    rclcpp::Publisher<basic_sprayer_interfaces::msg::Adc>::SharedPtr publisher_;
    basic_sprayer_interfaces::msg::Adc message;
    /* 滤波器 */
    // Dsp::Filter *filter;   // 滤波器
    /* 读回来的数据 */
    int16_t voltage[2] = {0};
    double voltagex[2] = {0};
    double offset[2] = {0};     //偏移
    int64_t timestamp = 0;    //
    /* iio 设备*/
    struct iio_context *ctx;
    struct iio_device *adc;
    struct iio_channel *chna;
    struct iio_channel *chnb;
    struct iio_channel *time;
	struct iio_device *trigger;   // 触发设备
    struct iio_buffer  *rxbuf;    // 读取的buffer

    bool iio_init(void){
        ctx = iio_create_default_context();
        if(ctx == NULL) return false;
        if( iio_context_get_devices_count(ctx) <= 0 ){
            perror("No devices\n");
            return false;
        }
        /* 寻找设备 */
        adc = iio_context_find_device(ctx, "ads1015");
        if(adc == NULL){
            perror("didn't find ads1015\n");
            return false;
        }
        /* 获取通道 */
        chna = iio_device_find_channel(adc, "voltage0", false);     //寻找输入通道
        if(chna == NULL){
            perror("didn't find ads1015 voltage0 channel\n"); 
            return false;
        }
        /* 设置通道的参数 */
        if( iio_channel_attr_write_longlong(chna,"sampling_frequency",2400) < 0 ){
            perror("sampling_frequency error\n"); 
            return false;
        }
        if(iio_channel_attr_write_double(chna,"scale",2) < 0){
            perror("scale error\n"); 
            return false;
        }
        chnb = iio_device_find_channel(adc, "voltage1", false);     // 寻找输入通道
        if(chna == NULL){
            perror("didn't find ads1015 voltage1 channel\n"); 
            return false;
        }
        /* 设置通道的参数 */
        if( iio_channel_attr_write_longlong(chnb,"sampling_frequency",2400) < 0 ){
            perror("sampling_frequency error\n"); 
            return false;
        }
        if(iio_channel_attr_write_double(chnb,"scale",2) < 0){
            perror("scale error\n"); 
            return false;
        }

        time = iio_device_find_channel(adc, "timestamp", false);     // 寻找输入通道
        if(time == NULL){
            perror("didn't find ads1015 timestamp channel\n"); 
            return false;
        }
        trigger = iio_context_find_device(ctx, "instance1");
        if (!trigger || !iio_device_is_trigger(trigger)) {
            perror("No trigger found (try setting up the iio-trig-hrtimer module)");
            return false;
        }
        /* 使能设备 */
        iio_channel_enable(chna);
        iio_channel_enable(chnb);   
        iio_channel_enable(time);   
        if ( iio_device_set_trigger(adc, trigger) ) {
		    perror("Could not set trigger\n");
            return false;
	    }
        rxbuf = iio_device_create_buffer(adc, 1, false);
        if ( !rxbuf ) {
            perror("Could not create buffer");
            return false;
        }        
        return true;
    }


    /* 关闭设备 */
    void shutdown(void){
        if (rxbuf) iio_buffer_destroy(rxbuf);
        if (chna)  iio_channel_disable(chna);
        if (chnb)  iio_channel_disable(chnb);
        if (time)  iio_channel_disable(time);
        if (ctx)   iio_context_destroy(ctx);
    }

    /* 接管强制退出信号 */
    int take_signals(void){
        struct sigaction sa;
        memset(&sa, 0, sizeof(sa));
        sigset_t mask;
        memset(&mask,0,sizeof(sigset_t));    //要手动清空

        sa.sa_handler = sig_handler;
        sigemptyset(&sa.sa_mask);
        sigemptyset(&mask);

        if (sigaction(SIGTERM, &sa, NULL) < 0) {
            printf("sigaction: %s\n", strerror(errno));
            return -1;
        }

        if (sigaction(SIGINT, &sa, NULL) < 0) {
            printf("sigaction: %s\n", strerror(errno));
            return -1;
        }

        sigaddset(&mask, SIGINT);
        sigaddset(&mask, SIGTERM);
        /* make sure these signals are unblocked */
        if (sigprocmask(SIG_UNBLOCK, &mask, NULL)) {
            printf("sigprocmask: %s", strerror(errno));
            return -1;
        }
        return 0;
    }
    void ros_publish(void){
        rclcpp::Time time(timestamp);             // 获取传感器读取时的时间戳
        // message.header.stamp = this->now();    //时间戳
        message.header.stamp = time;              // 时间戳
        message.channle_raw[0] = voltage[0];
        message.channle_raw[1] = voltage[1];
        /* 暂时不滤波 */
        voltagex[0] = voltage[0]*2.0/1000;//得到电压
        voltagex[1] = voltage[1]*2.0/1000;
        message.voltage_filter[0] = voltagex[0];
        message.voltage_filter[1] = voltagex[1];
        /* 得到角度 系数的确定 */
        /*
            最大电压3.3v 对应的角度是360deg
            所以系数为  360/3.3
        */
        #define  COFF (360/3.3) 
        message.angle_deg[0] = (voltagex[0] - offset[0])*COFF;   //得到角度
        message.angle_deg[1] = (voltagex[1] - offset[1])*COFF;

        //RCLCPP_INFO(rclcpp::get_logger("adc"), "raw value is [%d %d]", voltage[0], voltage[1]);
        publisher_->publish(message);     //发布
    }
  
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    // AdcAnglePublisher adc;
    auto pub_node = std::make_shared<AdcAnglePublisher>();
    pub_node->run();     //  不断运行
    rclcpp::shutdown();
    return 0;
}



